
#ifndef _MTSPEED_CALCULATE_H_
#define _MTSPEED_CALCULATE_H_

#include "stdint.h"
#include "arm_math.h"
#include "speed_calculate.h"


#define MT_CAL_TIME_CNT_MAX   60000
#define MT_CAL_TIME_S_MAX     (float32_t)(64e-3)
#define MT_CAL_TIME_SF        (float32_t)((1.0f / (float32_t) MT_CAL_TIME_CNT_MAX) * MT_CAL_TIME_S_MAX)

#define MT_CAL_SPEED_SF       (float32_t)(1.0f / (float32_t)MAGNET_ENCODER_LINE_NUM)

typedef enum
{
	MT_STOP = 0,

}MT_STATUS;

typedef enum
{
	MT_MOTOR_LEFT = 0,
	MT_MOTOR_RIGHT,

}MT_MOTOR_NUM;

typedef struct
{
	MT_STATUS FSM;
	
	uint16_t timer_cnt_now;
	uint16_t timer_cnt_old;
	uint16_t timer_cnt_diff;
	
	uint16_t eqep_cnt_now;
	uint16_t eqep_cnt_old;
	int16_t eqep_cnt_diff;
	
	uint8_t ISR_enable;
	uint8_t speed_update;
	
	float32_t speed_rps;
	float32_t speed_rps_lpf;
	float32_t timer_sf;

}MT_SPDCAL;


extern MT_SPDCAL MT_calculate[2];


extern void MTspeedcal_init(void);
extern void MTspeedcal_start(void);

#endif


